package pl.elka.simulator.core;

import java.util.List;

public class Constraint implements IPaintable{
	protected List<Position> edges;
	private Segment[] edgeSegments;
	public float minX, maxX, minY, maxY;

	public Constraint() {
		super();
	}

	public Constraint(List<Position> edges) {
		super();
		this.edges = edges;
		countOptimalizationHints();
	}

	public List<Position> getEdges() {
		return edges;
	}

	public void setEdges(List<Position> edges) {
		this.edges = edges;
		countOptimalizationHints();
	}

	public Segment findCollisionSegment(Position begin, Position end) {
		Position nearestColisionPoint = null;
		Segment nearestCollisionSegment = null;
		for(Segment edge : edgeSegments){
			Position colisionPoint=findCollision(begin,end,edge);
			if(colisionPoint!=null){
				nearestColisionPoint = chooseNearerPosition(begin,nearestColisionPoint,colisionPoint);			
				if(nearestColisionPoint == colisionPoint){//zmienil sie najblizszu punkt
					nearestCollisionSegment = edge;
				}
			}
		}
		if(nearestColisionPoint!=null){
			return nearestCollisionSegment;
		}
//		//bo czasem sie zdarza, ze po wpadnieciu na przszkode, w kolejnej iteracji wpada na nia znowu
//		if(contains(end)){
//			return begin;
//		}
		return null;
	}
	
	public Position findCollisionPoint(Position begin, Position end){
		Position nearestColisionPoint = null;
		for(Segment edge : edgeSegments){
			Position colisionPoint=findCollision(begin,end,edge);
			if(colisionPoint!=null){
				nearestColisionPoint = chooseNearerPosition(begin,nearestColisionPoint,colisionPoint);						
			}
		}
		if(nearestColisionPoint!=null){
			return nearestColisionPoint;
		}
		//bo czasem sie zdarza, ze po wpadnieciu na przszkode, w kolejnej iteracji wpada na nia znowu
		if(contains(end)){
			return begin;
		}
		return null;
	}
	
	public static Position chooseNearerPosition(Position nearTo,Position pos1, Position pos2) {
		if(pos1==null){
			return pos2;
		}
		if(pos2==null){
			return pos1;
		}
		//nie ma sensu liczyc pierwiastkow
		float d1 = (nearTo.x-pos1.x)*(nearTo.x-pos1.x) + (nearTo.y-pos1.y)*(nearTo.y-pos1.y);
		float d2 = (nearTo.x-pos2.x)*(nearTo.x-pos2.x) + (nearTo.y-pos2.y)*(nearTo.y-pos2.y);
		
		if(d1<=d2){
			return pos1;
		}		
		else{
			return pos2;
		}
	}

	private Position findCollision(Position e1, Position e2, Segment edge) {
		float d1 = det(e1,e2,edge.begin);
		float d2 = det(e1,e2,edge.end);
		float d3 = det(edge.begin,edge.end,e1);
		float d4 = det(edge.begin,edge.end,e2);
		
//		System.out.println("d1:" + d1);
//		System.out.println("d2:" + d2);
//		System.out.println("d3:" + d3);
//		System.out.println("d4:" + d4);
		
		if(d1*d2<0 && d3*d4<0){
			return crosscut(e1,e2, edge.begin,edge.end);
		}
		if(0.000001f<d1 && d1<0.000001f && onSegment(e1,e2,edge.begin)){
			return crosscut(e1,e2, edge.begin,edge.end);
		}
		if(0.000001f<d2 && d2<0.000001f && onSegment(e1,e2,edge.end)){
			return crosscut(e1,e2, edge.begin,edge.end);
		}
		if(0.000001f<d3 && d3<0.000001f && onSegment(edge.begin,edge.end,e1)){
			return crosscut(e1,e2, edge.begin,edge.end);
		}
		if(0.000001f<d4 && d4<0.000001f && onSegment(edge.begin,edge.end,e2)){
			return crosscut(e1,e2, edge.begin,edge.end);
		}
		
		return null;
	}

	private Position crosscut(Position l1p1,Position l1p2, Position l2p1, Position l2p2){
		if(l1p1.x == l1p2.x){
    		float x = l1p2.x;
    		float a2 = (l2p1.y - l2p2.y)/(l2p1.x - l2p2.x);	
    		float b2 = l2p1.y - a2*l2p1.x;
    		float y = a2*x+b2;
    		return new Position(x,y);
    	}
    	if(l2p1.x == l2p2.x){
    		float x = l2p2.x;
    		float a1 = (l1p1.y - l1p2.y)/(l1p1.x - l1p2.x);	
    		float b1 = l1p1.y - a1*l1p1.x;
    		float y = a1*x+b1;
    		return new Position(x,y);
    	}
    	
		float a1 = (l1p1.y - l1p2.y)/(l1p1.x - l1p2.x);
		float a2 = (l2p1.y - l2p2.y)/(l2p1.x - l2p2.x);
		float b1 = l1p1.y - a1*l1p1.x;
		float b2 = l2p1.y - a2*l2p1.x;
		
		float w = a2 - a1;
		float wx = b1 - b2;
		float wy = a2*b1 - a1*b2;
		
		return new Position(wx/w,wy/w);
	}
	private boolean onSegment(Position p1, Position p2, Position p3) {
		return
				Math.min(p1.x, p2.x)<=p3.x 
				&& p3.x <= Math.max(p1.x, p2.x)
				&& Math.min(p1.y, p2.y)<=p3.y
				&& p3.y<=Math.max(p1.y, p2.y);
	}

	private float det(Position p1, Position p2, Position p3) {
		return p1.x*p2.y + p2.x*p3.y + p3.x*p1.y - p3.x*p2.y - p1.x*p3.y - p2.x*p1.y;
	}

	public boolean contains(Position tp) {
		return contains(tp.getX(), tp.getY());
	}
	public boolean contains(float px,float py) {
		
		if(maxX<px
			|| minX>px
			|| maxY<py
			|| minY>py){					
			return false;
		}
		
		boolean c = false;
		int nvert = edges.size();
		Position vertC = null;
		Position vertP = null;
		
		for (int i = 0, j = nvert-1; i < nvert; j = i++) {
			vertC = edges.get(i);
			vertP = edges.get(j);

			if ( ((vertC.getY()>py) != (vertP.getY()>py)) &&
				(px < (vertP.getX()-vertC.getX()) * (py-vertC.getY()) / (vertP.getY()-vertC.getY()) + vertC.getX()) )
			   c = !c;
		}
		
		return c;
	}
	
	private void countOptimalizationHints(){
		countEdgeSegments();
		countMinMAx();
	}
	
	private void countEdgeSegments(){
		edgeSegments = new Segment[edges.size()];
		for(int i=1;i<=edges.size();++i){
			if(i<edges.size()){
				edgeSegments[i-1] = new Segment(edges.get(i-1),edges.get(i));
			}
			else{
				edgeSegments[i-1] = new Segment(edges.get(i-1),edges.get(0));
			}
		}
	}
	
	protected void countMinMAx(){
		maxY = maxX = Float.NEGATIVE_INFINITY;
		minY = minX = Float.POSITIVE_INFINITY;
		
		for(Position p:edges){
			if(p.getX()>maxX){
				maxX = p.getX();
			}
			if(p.getY()>maxY){
				maxY = p.getY();
			}
			if(p.getX()<minX){
				minX = p.getX();
			}
			if(p.getY()<minY){
				minY = p.getY();
			}
		}
	}
	
}
